import pybullet as p
from time import sleep

physicsClient = p.connect(p.GUI)
import pybullet_data

p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)

gravZ = -10
p.setGravity(0, 0, gravZ)

planeOrn = [0, 0, 0, 1]  # p.getQuaternionFromEuler([0.3,0,0])
# planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)

boxId = p.loadURDF("cube.urdf", [0, 1, 2], useMaximalCoordinates=True)

clothId = p.loadSoftBody("cloth_z_up.obj", basePosition=[0, 0, 2], scale=0.5, mass=1., useNeoHookean=0,
                         useBendingSprings=1, useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1,
                         springDampingAllDirections=1, useSelfCollision=0, frictionCoeff=.5, useFaceContact=1)

p.changeVisualShape(clothId, -1, flags=p.VISUAL_SHAPE_DOUBLE_SIDED)

p.createSoftBodyAnchor(clothId, 24, -1, -1)
p.createSoftBodyAnchor(clothId, 20, -1, -1)
p.createSoftBodyAnchor(clothId, 15, boxId, -1, [0.5, -0.5, 0])
p.createSoftBodyAnchor(clothId, 19, boxId, -1, [-0.5, -0.5, 0])
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)

debug = True
if debug:
    data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
    print("--------------")
    print("data=", data)
    print(data[0])
    print(data[1])
    text_uid = []
    for i in range(data[0]):
        pos = data[1][i]
        uid = p.addUserDebugText(str(i), pos, textColorRGB=[1, 1, 1])
        text_uid.append(uid)

while p.isConnected():
    p.getCameraImage(320, 200)

    if debug:
        data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
        for i in range(data[0]):
            pos = data[1][i]
            uid = p.addUserDebugText(str(i), pos, textColorRGB=[1, 1, 1], replaceItemUniqueId=text_uid[i])

    p.setGravity(0, 0, gravZ)
    p.stepSimulation()